'''
1.0.0 版本 
# Date: 2024/12/28
# Author: ZHENG
# Description:
    # 此版本功能如下：
        1. 机械臂控制(Mirobot ROS功能包)
        2. 小车状态显示、控制
        3. 小车地图显示
        4. 小车路径规划
        5. 小车轨迹显示
        6. 小车避障

        
# Modify 2025/5/27
1. 修改地图显示：增加服务端代码，订阅oddm和map话题数据，传输给客户端显示
'''

# -*- coding: utf-8 -*-
import sys
import os
import signal
from functools import partial
from PyQt5.QtWidgets import QApplication, QMainWindow, QWidget, QGraphicsProxyWidget, QGraphicsScene, QGraphicsView, QSizePolicy, QVBoxLayout
from PyQt5 import uic
from PyQt5.QtCore import Qt, QCoreApplication, QEvent, QRectF
from PyQt5.QtGui import QKeyEvent, QPainter, QColor, QFont, QPen, QLinearGradient, QTextOption, QIcon, QImage
import tkinter as tk
from tkinter import messagebox
import struct
import json

import subprocess
import time
import numpy as np
from client_ import ROS_Client
from threading import Thread
### web显示地图的方式--V1.0.1再引入
# from rviz_ import rvizWindow
from pmg_map import DisplayCostMap
from pmg_map import Vehicle
# from cost_map import DisplayCostMap
# from cost_map import OccupancyMap
from nav_new import NavGoalTableView

current_dir = os.path.dirname(os.path.abspath(__file__))
pgmFile_path = os.path.join(current_dir, "..", "maps", "default.pgm")

class MainWindow(QMainWindow):
    
    def __init__(self):
        super().__init__()
        self.host = '192.168.10.100'
        self.port = 8888
        self.setupUI()
        self.initUI()
        self.signal_thread = Thread(target=self.initSignals())
        self.signal_thread.start()
        # self.initSignals()
        ### 绘画速度、电量、温度
        # self.dashBoard_show()
        ### designer设计的速度、电量、温度
        self.dashBoard_show2()
        ### costmap 绘制坐标点
        self.costMap_x = 50
        self.costMap_y = 50
        ### costmap
        # self.mapNavigate_show()
        ### pgm地图
        # self.mapNavigate_pgmShow()
        self.label_mapName.setText("default.pgm")

        # 按钮状态跟踪
        self.button_states = {
            1: False, 2: False, 3: False, 4: False,
            5: False, 6: False, 7: False, 8: False
        }


        self.linear_speed = 1
        self.angular_speed = 0.5
        self.is_omni_mode = False

        self.map_info = {
            'width': 0,
            'height': 0,
            'resolution': 0.05,
            'origin_x': 0.0,
            'origin_y': 0.0
        }
        self.vehicle_pos = (0, 0, 0)  # (x, y, theta)
        
        time.sleep(2)
        ### 客户端连接状态
        self.clientStatus = False
        self.updateRobotStatus_thread = Thread(target=self.updateRobotStatuLabel)
        self.updateRobotStatus_thread.start()
        # 启动TCP监听线程
        # self.start_tcp_listener()

    def start_tcp_listener(self):
        import threading
        def listener_thread():
                     while self.ros_client.pmg_flag:
                        data = self.ros_client.receive_data
                        self.process_received_data(data)
                        # if data.startswith(b'ODOM'):
                        #     self.process_odom_data(data[4:])
                        # else:
                        #     self.process_map_data(data)
        
        threading.Thread(target=listener_thread, daemon=True).start()

    def process_map_data(self, data):
        # 解析地图头信息（28字节头）
        header = data[:28]
        width, height, res, ox, oy, oz = struct.unpack('!IIddd', header)
        self.map_info.update({
            'width': width,
            'height': height,
            'resolution': res,
            'origin_x': ox,
            'origin_y': oy
        })
        
        # 处理地图数据
        map_array = list(bytearray(data[28:]))
        self.update_map_display(map_array)

    def process_odom_data(self, data):
        x, y, theta = struct.unpack('!ddd', data)
        self.vehicle_pos = (x, y, theta)
        self.update_vehicle_position()

    def update_map_display(self, map_array):
        # 创建QImage显示地图
        width = self.map_info['width']
        height = self.map_info['height']
        image = QImage(width, height, QImage.Format_Grayscale8)
        
        for y in range(height):
            for x in range(width):
                value = map_array[y * width + x]
                # 颜色映射
                if value == -1:    # 未知区域
                    color = 128
                elif value == 0:   # 自由空间
                    color = 255
                else:              # 障碍物
                    color = 0
                image.setPixel(x, y, color)
        
        # 在DisplayCostMap中显示
        if hasattr(self, 'mapNavigate'):
            self.mapNavigate.set_map_image(image)
            self.mapNavigate.update()

    def update_vehicle_position(self):
        if hasattr(self, 'mapNavigate'):
            self.mapNavigate.set_vehicle_pos(*self.vehicle_pos)
            self.mapNavigate.update()

    ### 弹窗提示信息
    def show_popup(self, title, message):
        root = tk.Tk()
        root.withdraw()
        messagebox.showinfo(title, message)
        root.destroy()

    def setupUI(self):
        current_dir = os.path.dirname(os.path.abspath(__file__))
        ui_file_path = os.path.join(current_dir, "..", "ui", "mainWindow.ui")
        self.ui = uic.loadUi(ui_file_path, self)
        self.setWindowTitle("ZBot Monitor")
        icon_path = os.path.join(current_dir, "..", "images", "icon.png")
        self.setWindowIcon(QIcon(icon_path))

    def initUI(self):
        self.frame_dashBoard = self.ui.frame_dashBoard
        self.frame_middle = self.ui.frame_middle
        self.page_navM = self.ui.page_navM
        ### 地图导航菜单栏
        self.page_navM_Point1 = self.ui.page_navM_Point1
        self.pushButton_carNav = self.ui.pushButton_carNav
        self.pushButton_pointA = self.ui.pushButton_pointA
        self.pushButton_pointB = self.ui.pushButton_pointB

        ### 地图管理菜单栏
        self.pushButton_manageMaps = self.ui.pushButton_manageMaps
        self.pushButton_saveMap = self.ui.pushButton_saveMap
        self.pushButton_openMap = self.ui.pushButton_openMap

        self.pushButton_mapping2D = self.ui.pushButton_mapping2D

        self.lineEdit_ip = self.ui.lineEdit_ip
        self.lineEdit_port = self.ui.lineEdit_port
        self.lineEdit_ip.setText(self.host)
        self.lineEdit_port.setText(str(self.port))

        self.pushButton_conn = self.ui.pushButton_conn
        self.pushButton_disconnect = self.ui.pushButton_disconnect

        self.buttons = {
            1: self.ui.pushButton_carManage,
            2: self.ui.pushButton_sensorManage,
            3: self.ui.pushButton_manageMaps,
            4: self.ui.pushButton_robotManage
        }

        self.direct_buttons = {
            1: self.ui.pushButton_up_left,
            2: self.ui.pushButton_up,
            3: self.ui.pushButton_up_right,
            4: self.ui.pushButton_left,
            5: self.ui.pushButton_right,
            6: self.ui.pushButton_down_left,
            7: self.ui.pushButton_down,
            8: self.ui.pushButton_down_right
        }

        self.robot_control_buttons = {
            1:self.ui.pushButton_J1_add,
            2:self.ui.pushButton_J1_min,
            3:self.ui.pushButton_J2_add,
            4:self.ui.pushButton_J2_min,
            5:self.ui.pushButton_J3_add,
            6:self.ui.pushButton_J3_min,
            7:self.ui.pushButton_J4_add,
            8:self.ui.pushButton_J4_min,
            9:self.ui.pushButton_J5_add,
            10:self.ui.pushButton_J5_min,
            11:self.ui.pushButton_J6_add,
            12:self.ui.pushButton_J6_min
        }

        self.robot_joint_label = {
            1: self.label_robot_J1,
            2: self.label_robot_J2,
            3: self.label_robot_J3,
            4: self.label_robot_J4,
            5: self.label_robot_J5,
            6: self.label_robot_J6
        }

        self.pushButton_robotHome = self.ui.pushButton_robotHome
        self.pushButton_robotStop = self.ui.pushButton_robotStop
        self.pushButton_robotZero = self.ui.pushButton_robotZero
        self.label_robotType = self.ui.label_robotType
        self.label_robotStatus = self.ui.label_robotStatus

        self.pushButton_robotConnect = self.ui.pushButton_robotConnect
        
        self.stackedWidget = self.ui.stackedWidget
        self.stackedWidget_config = self.ui.stackedWidget_config

        self.car_control_buttons = {
            'start': self.ui.pushButton_carStart,
            'stop': self.ui.pushButton_carStop
        }

    def initSignals(self):
        for index, button in self.buttons.items():
            button.clicked.connect(partial(self.on_pushButton_Manage_clicked, index))

        self.car_control_buttons['start'].clicked.connect(self.on_pushButton_carStart_clicked)
        self.car_control_buttons['stop'].clicked.connect(self.on_pushButton_carStop_clicked)

        for index, button in self.robot_control_buttons.items():
            button.clicked.connect(partial(self.on_pushButton_robotControl_clicked, index))

        for index, button in self.direct_buttons.items():
            ### button.clicked.connect(partial(self.on_pushButton_direct_clicked, index))
            button.pressed.connect(partial(self.on_direct_button_pressed, index))
            button.released.connect(partial(self.on_direct_button_released))

        for index, label in self.robot_joint_label.items():
            label.setText(str(0))
            label.setAlignment(Qt.AlignCenter)
        
        self.label_robotType.setText("KT-6")
        self.label_robotStatus.setText("Offline")
        self.pushButton_robotStop.clicked.connect(lambda: self.on_pushButton_robotControl_clicked(index=21))
        self.pushButton_robotHome.clicked.connect(lambda: self.on_pushButton_robotControl_clicked(index=22))
        self.pushButton_robotZero.clicked.connect(lambda: self.on_pushButton_robotControl_clicked(index=23))
        self.pushButton_robotConnect.clicked.connect(lambda: self.on_pushButton_robotControl_clicked(index=24))


        self.pushButton_conn.clicked.connect(self.on_pushButton_connect_clicked)
        self.pushButton_disconnect.clicked.connect(self.on_pushButton_disconnect_clicked)

        # self.pushButton_manageMaps.clicked.connect(self.on_pushButton_mapManage_clicked)
        self.pushButton_mapping2D.clicked.connect(self.on_pushButton_mapManage_clicked)
        self.pushButton_saveMap.clicked.connect(self.on_pushButton_saveMaps_clicked)
        self.pushButton_openMap.clicked.connect(self.on_pushButton_openMaps_clicked)

        self.pushButton_carNav.clicked.connect(self.on_pushButton_carNav_clicked)
        self.pushButton_pointA.clicked.connect(self.on_pushButton_points_clicked)

    def on_direct_button_pressed(self, index):
        """方向按钮按下时发送移动命令"""
        # if self.last_direction == index:
        #     return
        self.last_direction = index
        self.send_move_command(index)

    def on_direct_button_released(self):
        """方向按钮释放时发送停止命令"""
        pass
        # self.send_stop_command()

    def send_move_command(self, direction_index):
        """发送移动命令到ROS"""
        if not hasattr(self, 'ros_client') or not self.ros_client.client_connected:
            self.show_popup("连接错误", "未连接到ROS服务器")
            return
        
        # 根据方向索引确定移动命令
        commands = {
            1: 'u',  # 左上
            2: 'i',  # 上
            3: 'o',  # 右上
            4: 'j',  # 左
            5: 'l',  # 右
            6: 'm',  # 左下
            7: ',',  # 下
            8: '.'   # 右下
        }
        
        cmd_char = commands.get(direction_index, 'k')  # 默认为停止
        
        # 创建命令字符串
        cmd_str = f"MOVE:{cmd_char}:{self.linear_speed}:{self.angular_speed}:{int(self.is_omni_mode)}"
        
        # 通过TCP发送命令
        try:
            self.ros_client.send_direct_command(cmd_str.encode('utf-8'))
        
        except Exception as e:
            print(f"发送移动命令失败: {e}")

    def send_stop_command(self):
        """发送停止命令"""
        if hasattr(self, 'ros_client') and self.ros_client.client_connected:
            try:
                self.ros_client.send_direct_command(b"MOVE:k:0:0:0")
            
            except Exception as e:
                print(f"发送停止命令失败: {e}")

    def set_button_style(self, button, active):
        style = "background-color: rgb(115, 152, 163);" if active else ""
        button.setStyleSheet(style)

    def dashBoard_show(self):
        try:
            self.dashboard = Dashboard(self.frame_dashBoard)
            self.dashboard.setGeometry(0,0,200,378)
            self.dashboard.show()

            set_parameters_thread = Thread(target=self.set_rosBridge_parameters)
            set_parameters_thread.start()

        except Exception as e:
            print(f"dashBoard_show error: {e}")

    ### 设计的速度、电量、温度,初始化
    def dashBoard_show2(self):
        try:
            self.label_showSpeed = self.ui.label_showSpeed
            self.label_showSpeed.setText("0")
            self.label_showSpeed.setAlignment(Qt.AlignCenter)

            self.label_showPower = self.ui.label_showPower
            self.label_showPower.setText("100")
            self.label_showPower.setAlignment(Qt.AlignCenter)

            self.label_showTemperature = self.ui.label_showTemperature
            self.label_showTemperature.setText("30")
            self.label_showTemperature.setAlignment(Qt.AlignCenter)

        except Exception as e:
            print(f"dashBoard_show error: {e}")

    ### 地图导航窗口 debug version
    def mapNavigate_show(self):
        try:
            # self.mapNavigate = rvizWindow(self.frame_middle)
            # self.mapNavigate.setGeometry(0,0,730,760)
            # self.mapNavigate.show()
            self.scene = QGraphicsScene(self)
            self.graphicsView.setScene(self.scene)

            self.mapNavigate = DisplayCostMap()
            proxy_widget = QGraphicsProxyWidget()
            proxy_widget.setWidget(self.mapNavigate)
            self.scene.addItem(proxy_widget)

            # self.mapNavigate.setGeometry(0,0,730,760)
            self.mapNavigate.show()
            self.update_display()

        except Exception as e:
            print(f"mapNavigate_show error: {e}")

    ### tcp数据pgm地图导航窗口 new
    def mapNavigate_pgmShow(self):
        try:
            self.mapNavigate = DisplayCostMap(self.frame_mapShow)
            self.mapNavigate.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
            self.mapNavigate.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)

            ## 设置尺寸策略为扩展
            self.mapNavigate.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)

            ## 如果 frame_mapShow 没有布局，创建一个新的布局并添加 DisplayCostMap 到该布局
            if not self.frame_mapShow.layout():
                layout = QVBoxLayout(self.frame_mapShow)
                layout.setContentsMargins(0, 0, 0, 0)
                layout.addWidget(self.mapNavigate)

            self.update_display()

        except Exception as e:
            print(f"mapNavigate_show error: {e}")

    ### 创建一个模拟的成本地图数据
    def update_display(self):
        # width, height = 640, 480
        # data = np.random.randint(0, 256, (height, width, 4), dtype=np.uint8)
        # # occupancy_map = OccupancyMap(data)

        ### 更新DisplayCostMap的数据
        # self.mapNavigate.update_data(occupancy_map, vehicle_pos=(self.costMap_x, self.costMap_y))
        vehicle_position = (self.costMap_x, self.costMap_y)
        self.mapNavigate.update_data(pgmFile_path, vehicle_position)

    def navGoal_show(self):
        try:
            # if not hasattr(self, 'navGoal') or self.navGoal is None:
            #     self.navGoal = NavGoalTableView(self.page_navM)
            #     # 使用布局管理器来管理 NavGoalTableView 的位置和大小
            #     layout = QVBoxLayout(self.page_navM)
            #     layout.addWidget(self.navGoal)
            # else:
            #     self.navGoal.show()

            self.navGoal = NavGoalTableView(self.page_navM_Point1)
            self.navGoal.setGeometry(0,0,300,760)
            self.navGoal.show()

        except Exception as e:
            print(f"navGoal_show error: {e}")

    def set_rosBridge_parameters(self):
        try:
            if self.ros_client.client_connected:
                speed = self.ros_client.ros_speed
                temperature = 45
                battery_level = self.ros_client.ros_battery
                if self.ros_client.ros_angle >= 0 and self.ros_client.ros_angle <= 180:
                    gear = "D"
                elif self.ros_client.ros_angle < 0 and self.ros_client.ros_angle >= -180:
                    gear = "R"
                else:
                    gear = "N"

            else:
                speed = 0
                temperature = 45
                battery_level = 80
                gear = "N"
            
            self.dashboard.set_speed(speed)
            self.dashboard.set_temperature(temperature)
            self.dashboard.set_battery_level(battery_level)
            self.dashboard.set_gear(gear)
        
        except Exception as e:
            print(f"set_rosBridge_parameters error: {e}")

    def start_mobaxterm_vnc_session(self, host, port=5901):
        # MobaXterm可执行文件路径，假设它安装在默认位置
        mobaxterm_path = r'"D:\Programs\Mobatek\MobaXterm\MobaXterm.exe"'
        
        # 构建命令字符串
        command = f'{mobaxterm_path} -vnc -host {host} -port {port}'
        
        try:
            # 使用subprocess.Popen启动MobaXterm VNC会话
            process = subprocess.Popen(command, shell=True)
            print(f"Starting VNC session with host: {host}, port: {port}")
            
            # 等待进程结束，这里可以根据需要选择是否等待
            # process.wait()
            
        except Exception as e:
            print(f"An error occurred: {e}")

    def on_pushButton_mapManage_clicked(self):
        try:
            # ### Linux (Ros)
            # subprocess.Popen(['xfce4-terminal', '--command', 'bash -c "rviz; exec bash"', '--hold'])

            ## Windows
            self.start_mobaxterm_vnc_session('192.168.1.127', 5901)
            time.sleep(1)
            if self.ros_client:
                self.ros_client.send_command(ros_run="rviz")
            else:
                # print(f"No ROS client connect")
                self.show_popup("TCP连接状态", "未连接ROS服务器")
                pass

        except Exception as e:
            print(f"on_pushButton_mapManage_clicked error: {e}")

    ### 打开地图
    def on_pushButton_openMaps_clicked(self):
        try:
            ### Linux (Ros)
            # subprocess.Popen(['xfce4-terminal', '--command', 'bash -c "rviz; exec bash"', '--hold'])
            
            # subprocess.Popen(['xfce4-terminal', '--command', 'bash -c "roslaunch zbot3_drive zbot3_navigation.launch; exec bash"', '--hold'])
            
            ## Windows
            if self.ros_client:
                self.ros_client.pmg_flag = True
                time.sleep(2)
                # self.start_tcp_listener()
                # magic_number, width, height, max_gray_value, pixels = self.read_pgm(pgmFile_path)
                data = self.ros_client.receive_data()
                self.process_received_data(data)

                self.show_popup("地图管理", "打开地图成功")
            else:
                # print(f"No ROS client connect")
                self.show_popup("TCP连接状态", "未连接ROS服务器")
                pass
        
        except Exception as e:
            print(f"on_pushButton_openMap_clicked error: {e}")

    def process_received_data(self, data):
        """处理从ROS服务器接收的数据"""
        if data.startswith(b'MAP'):
            self.process_map_data(data[3:])
        elif data.startswith(b'ODOM'):
            self.process_odom_data(data[4:])

    def process_map_data(self, data):
        """处理地图数据"""
        try:
            # 解析地图头信息（32字节头）
            if len(data) < 32:
                return
                
            header = data[:32]
            width, height, res, ox, oy = struct.unpack('!IIddd', header)
            
            # 更新地图信息
            self.map_info.update({
                'width': width,
                'height': height,
                'resolution': res,
                'origin_x': ox,
                'origin_y': oy
            })
            
            # 处理地图数据
            map_array = data[32:32+width*height]
            self.update_map_display(map_array)
            
        except Exception as e:
            print(f"Map data processing error: {e}")

    def process_odom_data(self, data):
        """处理里程计数据"""
        try:
            if len(data) >= 24:  # 3个double类型，每个8字节
                x, y, theta = struct.unpack('!ddd', data[:24])
                self.vehicle_pos = (x, y, theta)
                self.update_vehicle_position()
        except Exception as e:
            print(f"Odom data processing error: {e}")

    def update_map_display(self, map_array):
        """更新地图显示"""
        width = self.map_info['width']
        height = self.map_info['height']
        
        # 创建QImage显示地图
        image = QImage(width, height, QImage.Format_Grayscale8)
        
        # 填充像素数据
        for y in range(height):
            for x in range(width):
                idx = y * width + x
                if idx < len(map_array):
                    value = map_array[idx]
                    # 设置像素颜色
                    image.setPixel(x, y, value)
        
        # 更新地图控件
        if hasattr(self, 'mapNavigate'):
            self.mapNavigate.set_map_image(image)
            self.mapNavigate.update()


    def read_pgm(self, pgmFile_path):
        with open(pgmFile_path, 'rb') as f:
            magic_number = f.readline().decode().strip()
            # 跳过注释行（如果有的话）
            while magic_number.startswith('#'):
                magic_number = f.readline().decode().strip()

            # 读取宽度和高度，同时忽略注释行
            dimensions = []
            while len(dimensions) < 2:
                line = f.readline().decode().strip()
                if not line.startswith('#'):
                    dimensions.extend(line.split())
            width, height = map(int, dimensions)

            # 读取最大灰度值，同样需要忽略注释行
            max_gray_value = None
            while max_gray_value is None:
                line = f.readline().decode().strip()
                if not line.startswith('#'):
                    max_gray_value = int(line)

            pixels = []
            if magic_number == 'P5':
                # 二进制格式
                pixels = list(f.read())
            elif magic_number == 'P2':
                # ASCII格式
                for line in f:
                    stripped_line = line.decode().strip()
                    if stripped_line and not stripped_line.startswith('#'):
                        pixels.extend(map(int, stripped_line.split()))
            else:
                raise ValueError("Unsupported file type.")

            return (magic_number, width, height, max_gray_value, pixels)

    ### 保存地图
    def on_pushButton_saveMaps_clicked(self):
        try:
            # ### Linux (Ros)
            # terminal_command = ['xfce4-terminal', '--command', f'bash -c "timeout 10 roslaunch zbot3_drive zbot3_savemap.launch && exit"']
            # subprocess.Popen(terminal_command)
            
            ## Windows
            if self.ros_client:
                self.ros_client.send_command(ros_run="saveMap")
                self.show_popup("地图管理", "保存地图成功")
            else:
                # print(f"No ROS client connect")
                self.show_popup("TCP连接状态", "未连接ROS服务器")
                pass
        
        except Exception as e:
            print(f"on_pushButton_saveMap_clicked error: {e}")

    def on_pushButton_Manage_clicked(self, index):
        try:
            for i, button in self.buttons.items():
                self.set_button_style(button, i == index)
            self.stackedWidget.setCurrentIndex(index - 1)
            self.stackedWidget_config.setCurrentIndex(index - 1)

        except Exception as e:
            print(f"on_pushButton_Manage_clicked error: {e}")

    ### 小车导航参数配置
    def on_pushButton_carNav_clicked(self):
        try:
            # ### Linux (Ros)
            # terminal_command = ['xfce4-terminal', '--command', f'bash -c "timeout 10 roslaunch zbot3_drive zbot3_movebase.launch && exit"']
            # subprocess.Popen(terminal_command)
            # time.sleep(1)
            # subprocess.Popen(['xfce4-terminal', '--command', 'bash -c "rosrun rqt_gui rqt_gui"', '--hold'])
            
            ## Windos
            if self.ros_client:
                self.ros_client.send_command(ros_run="carNavConfig")
            else:
                # print(f"No ROS client connect")
                self.show_popup("TCP连接状态", "未连接ROS服务器")
                pass
        
        except Exception as e:
            print(f"on_pushButton_carNav_clicked error: {e}")

    ### 小车定点导航按钮响应
    def on_pushButton_points_clicked(self):
        try:
            # ### Linux (Ros)
            # terminal_command = ['xfce4-terminal', '--command', f'bash -c "roslaunch zbot3_drive zbot3_navigation.launch; exec bash"', '--hold']
            # # terminal_command = ['xfce4-terminal', '--command', f'bash -c "timeout 10 roslaunch zbot3_drive zbot3_navigation.launch && exit"']
            # subprocess.Popen(terminal_command)
            
            ## Windows
            if self.ros_client:
                self.ros_client.send_command(ros_run="pointA")
            else:
                # print(f"No ROS client connect")
                self.show_popup("TCP连接状态", "未连接ROS服务器")
                pass
        
        except Exception as e:
            print(f"on_pushButton_pointA_clicked error: {e}")


    ### 小车启动按钮响应
    def on_pushButton_carStart_clicked(self):
        try:
        #     ### Linux (Ros)
        #     terminal_command = ['xfce4-terminal', '--command', f'bash -c "roslaunch zbot3_drive zbot3_bringup.launch; exec bash"', '--hold']
        #     subprocess.Popen(terminal_command)
            
            ## Windows
            self.car_control_buttons['start'].setEnabled(False)
            self.car_control_buttons['stop'].setEnabled(True)
            ## 启动小车底盘
            launch_file = 'zbot3_bringup.launch'
            bag_name = "zbot3_drive"
            # launch_file = 'zbot3_demo.launch'
            # bag_name = "zbot3_navigation_new"
            self.ros_client.send_command(ros_launch = "roslaunch", ros_bag = bag_name, data = launch_file)
        
        except Exception as e:
            print(f"on_pushButton_carStart_clicked error: {e}")
    
    ### 小车停止按钮响应
    def on_pushButton_carStop_clicked(self):
        self.car_control_buttons['start'].setEnabled(True)
        self.car_control_buttons['stop'].setEnabled(False)
        # self.car_thread_2 = Thread(target=RosBridge.stopRos)
        # self.car_thread_2.start()

    ### 小车方向控制按钮响应
    def on_pushButton_direct_clicked(self, index):
        try:
            key_map = {
                1: (Qt.Key_U, Qt.NoModifier, 'u'),
                2: (Qt.Key_I, Qt.NoModifier, 'i'),
                3: (Qt.Key_O, Qt.NoModifier, 'o'),
                4: (Qt.Key_J, Qt.NoModifier, 'j'),
                5: (Qt.Key_L, Qt.NoModifier, 'l'),
                6: (Qt.Key_M, Qt.NoModifier, 'm'),
                7: (Qt.Key_Comma, Qt.ShiftModifier, ','),
                8: (Qt.Key_Period, Qt.ShiftModifier, '.'),
            }

            if index in key_map:
                self.send_key_event(*key_map[index])

        except Exception as e:
            print(f"on_pushButton_direct_clicked error: {e}")

    def send_key_event(self, key, modifiers, text):
        try:
            press_event = QKeyEvent(QEvent.KeyPress, key, modifiers, text)
            release_event = QKeyEvent(QEvent.KeyRelease, key, modifiers)

            QCoreApplication.postEvent(self.focusWidget(), press_event)
            QCoreApplication.postEvent(self.focusWidget(), release_event)
        
        except Exception as e:
            print(f"send_key_event error: {e}")
    
    ### 机械臂控制响应
    def on_pushButton_robotControl_clicked(self, index):
        try:
            # print(index)
            if index == 214:
                self.ros_client.send_command(ros_run="rosrun")
            else:
                gcode_map = {
                    1: 1,
                    2: 2,
                    3: 3,
                    4: 4,
                    5: 5,
                    6: 6,
                    7: 7,
                    8: 8,
                    9: 9,
                    10:10,
                    11:11,
                    12:12,
                    21:21,
                    22:22,
                    23:23
                }
                gcode = gcode_map.get(index, "")

            if index != 24:
                self.ros_client.send_command(ros_gcode="GCode", data= gcode)

        except Exception as e:
            self.show_popup("错误提示", "机械手控制失败原因:未连接")
            print(f"on_pushButton_robotControl_clicked error: {e}")

    def ensure_quoted(self, data):
        """Ensure the given data is quoted with double quotes."""
        if isinstance(data, str):
            # Check if data is already enclosed in double quotes
            if not (data.startswith('"') and data.endswith('"')):
                # Escape existing double quotes inside the string
                data = data.replace('"', r'\"')
                # Enclose the entire string in double quotes
                data = f'"{data}"'
        return data

    ### TCP连接按钮响应

    def on_pushButton_connect_clicked(self):
        try:
            self.host = self.lineEdit_ip.text().strip()
            self.port = self.lineEdit_port.text().strip()

            # 新增：空值检查
            if not self.host:
                raise ValueError("IP address cannot be empty")
            if not self.port:
                raise ValueError("Port cannot be empty")

            # 转换前先检查是否全是数字
            if not self.port.isdigit():
                raise ValueError("Port must be numeric")

            port = int(self.port)

            # 检查有效端口范围
            if not (0 < port <= 65535):
                raise ValueError("Port must be between 1-65535")

            # 创建客户端前可以添加调试输出
            print(f"Connecting to {self.host}:{port} (type: {type(port)})")

            self.ros_client = ROS_Client(server_ip=self.host, server_port=port)
            self.clientStatus = True
            self.ros_client.connect()
            

            # self.updateRobotStatuLabel()     

        except ValueError as ve:
            print(f"Input error: {ve}")
        
        except Exception as e:
            print(f"Connection error: {e}")

    def updateRobotStatuLabel(self):
        while True:
            time.sleep(0.5)
            if self.clientStatus:
                robotStatus = self.ros_client.ros_mirobotstate
                # print(robotStatus, type(robotStatus))
                if robotStatus == 0:
                    robotStatusText = "空闲"
                elif robotStatus == 1:
                    robotStatusText = "空闲"
                elif robotStatus == 2:
                    robotStatusText = "复位"
                elif robotStatus == 3:
                    robotStatusText = "回零"
                elif robotStatus == -1:
                    robotStatusText = "运行"
                else:
                    robotStatusText = "未知"
                
                self.label_robotStatus.setText(robotStatusText)


    ### TCP断开按钮响应
    def on_pushButton_disconnect_clicked(self):
        try:
            if self.ros_client:
                self.ros_client.close()
        
        except Exception as e:
            print(f"on_pushButton_disconnect_clicked error: {e}")



class Dashboard(QWidget):
    
    def __init__(self, parent=None):
        super().__init__(parent)
        self._gear = "D"
        self._speed = 0
        self._temperature = 25  # 默认温度值
        self._battery_level = 100  # 默认电池电量百分比

    def set_gear(self, gear):
        self._gear = gear
        self.update()

    def set_speed(self, speed):
        self._speed = speed
        self.update()

    def set_temperature(self, temperature):
        self._temperature = temperature
        self.update()

    def set_battery_level(self, battery_level):
        self._battery_level = max(0, min(100, battery_level))  # 确保电池水平在0到100之间
        self.update()

    def paintEvent(self, event):
        painter = QPainter(self)
        painter.setRenderHint(QPainter.Antialiasing)

        width = self.width()
        height = self.height()
        margin = 10  # 减少边距以适应更小的空间
        
        # Speedometer is placed in the center
        # speedometer_rect = QRectF(margin, margin, (width - 2 * margin), (height - 4 * margin) / 4)

        # Gear indicator is placed below the speedometer
        # gear_rect = QRectF(margin, speedometer_rect.bottom() + margin, (width - 2 * margin), 40)
        
        # # Temperature meter and Battery meter are placed at the bottom corners with labels above them
        # thermometer_rect = QRectF(margin, height - 30 , (width - 4 * margin) / 2, 105)
        # battery_meter_rect = QRectF(width / 2 + margin, height - 20 , (width - 4 * margin) / 2, 100)

        speedometer_rect = QRectF(margin, margin, (width - 2 * margin), 40)
        gear_rect = QRectF(margin, margin + 60, (width - 2 * margin), 40)
        thermometer_rect = QRectF(margin, margin + 220, (width - 4 * margin) / 2, 205)
        battery_meter_rect = QRectF(width / 2 + margin, margin + 235 , (width - 4 * margin) / 2, 200)

        self.draw_speedometer(painter, speedometer_rect)
        self.draw_gear(painter, gear_rect)
        self.draw_thermometer(painter, thermometer_rect)
        self.draw_battery_meter(painter, battery_meter_rect)

    def draw_speedometer(self, painter, rect):
        painter.save()
        
        # 设置背景线性渐变色
        gradient = QLinearGradient(rect.topLeft(), rect.bottomRight())
        gradient.setColorAt(0, QColor(70, 70, 70))
        gradient.setColorAt(1, QColor(50, 50, 50))
        painter.setBrush(gradient)
        painter.setPen(Qt.NoPen)
        
        # 绘制矩形框代替原来的椭圆
        painter.drawRect(rect)

        # 设置字体和颜色，并绘制文本
        font = QFont("Arial", 8, QFont.Bold)
        painter.setFont(font)
        painter.setPen(Qt.white)
        
        # 使用 QTextOption 来控制文本对齐方式
        text_option = QTextOption(Qt.AlignCenter)
        
        # 绘制速度值，确保文本在矩形框内居中显示
        painter.drawText(rect, f"速度： {self._speed} km/h", text_option)
        
        painter.restore()

    def draw_gear(self, painter, rect):
        painter.save()
        painter.setPen(QPen(Qt.white, 2))
        painter.setBrush(QColor(70, 70, 70))
        painter.drawRect(rect)

        font = QFont("Arial", 8, QFont.Bold)
        painter.setFont(font)
        painter.drawText(rect, Qt.AlignCenter, f"档位: {self._gear}")
        painter.restore()

    def draw_thermometer(self, painter, rect):
        painter.save()
        painter.setPen(QPen(Qt.red, 2))
        painter.setBrush(QColor(255, 0, 0, 127))
        # fill_height = rect.height() * (self._temperature / 100)
        fill_height = 150
        painter.drawRect(QRectF(rect.left(), rect.bottom() - fill_height, rect.width(), -fill_height))

        # Display temperature value above the thermometer with enough space for two lines of text
        label_rect = QRectF(rect.left(), rect.top() - 40, rect.width(), 40)  # Increase height to accommodate two lines
        painter.setPen(Qt.black)

        # Optionally, use QTextOption to control text alignment and wrapping
        text_option = QTextOption()
        text_option.setAlignment(Qt.AlignCenter)
        text_option.setWrapMode(QTextOption.WordWrap)  # Enable word wrap

        painter.drawText(label_rect, "温度\n{:.1f}°C".format(self._temperature), text_option)
        painter.restore()

    def draw_battery_meter(self, painter, rect):
        painter.save()
        painter.setPen(QPen(Qt.blue, 2))
        painter.setBrush(QColor(0, 255, 0, 127))  # Green for battery level
        # fill_height = rect.height() * (self._battery_level / 100)
        fill_height = 150
        painter.drawRect(QRectF(rect.left(), rect.bottom() - fill_height-5, rect.width(), -fill_height))

        # Display battery percentage above the battery meter with enough space for two lines of text
        label_rect = QRectF(rect.left(), rect.top() - 40, rect.width(), 40)  # Increase height to accommodate two lines
        painter.setPen(Qt.black)

        # Optionally, use QTextOption to control text alignment and wrapping
        text_option = QTextOption()
        text_option.setAlignment(Qt.AlignCenter)
        text_option.setWrapMode(QTextOption.WordWrap)  # Enable word wrap

        painter.drawText(label_rect, "电量\n{:.1f}%".format(self._battery_level), text_option)
        painter.restore()

    def resizeEvent(self, event):
        super().resizeEvent(event)
        self.update()




def signal_handler(sig, frame):
    QApplication.quit()

if __name__ == "__main__":
    app = QApplication(sys.argv)
    mainWindow = MainWindow()
    # mainWindow.setWindowFlags(
    #     mainWindow.windowFlags() & ~Qt.WindowMaximizeButtonHint
    # )
    mainWindow.show()
    signal.signal(signal.SIGINT, signal_handler)
    sys.exit(app.exec_())